#ifndef __TRACK_INITIALIZER_INCLUDE__
#define __TRACK_INITIALIZER_INCLUDE__
#include <iostream>  
#include <vector>
#include <map>
#include <functional>
#include "../UKF/UKF.h"
#include "../Eigen/Dense"
#include "TentativeTrack.h"
#include "FlickerTargetDetector.h"
#include "../plot_clott/PlotClottingCT.h"
using namespace Eigen;

typedef std::map<TraceIDType, std::vector<TentativeTrack> >		TraceInitSuccessMap_t;
typedef std::vector<TraceIDType>								DeleteTraceVec_t;
typedef std::map<TraceIDType, TentativeTrack>					TentativeTrackMap_t;
typedef std::function<void(TentativeTrack&)>					onTrackConfirmedAction;

//目标起始参数
class TrackInitializeParam {
public:
	TrackInitializeParam();
public:
	double minVel_ms;	//目标的最小速度 
	double maxVel_ms;	//目标的最大速度
	double m_r_err_std; //雷达距离量测误差(单位:m)
	double m_a_err_std;//雷达方位量测误差(单位:角度)
	double thetamax;	//点迹航向变化的最大值
	double dk;
	double staticObjSharkVel;	//静止的目标由于雷达误差而产生的抖动速度(单位:m/s)
	double max_acc_ms2;			//最大的加速度大小
	unsigned int track_formation_M;		//航迹初始逻辑M/N参数
	unsigned int track_formation_N;
};


class TrackInitializer {
public:
	TrackInitializer(onTrackConfirmedAction action = NULL);
	virtual ~TrackInitializer();

public:
	void updateAssociation(const CCVBlockVec_t& plots,
		double  T,
		int64_t current_timeSecFromEpoch,
		std::vector<size_t>& assignedStatus);

	const TrackInitializeParam& getTrackInitializeParam() const { return trackParam; }
	void setTrackInitializeParam(const TrackInitializeParam&);

public:
    void initTrackFilter(TentativeTrack& trace, double T);

    TentativeTrack addNewTrack(const CCVBlockInfo_t& plot, int64_t current_timeSecFromEpoch);
    void updateTrackByPlot(TentativeTrack& trace, const CCVBlockInfo_t& plot, double T, int64_t current_timeSecFromEpoch);
    bool confirmTrack(TentativeTrack& track);

	//计算K时刻的量测噪声（距离、方位）在直角坐标系下(XY)的标准差
	static void RAerr2XYerr(double r, double a, double m_r_err_std, double m_a_err_std, double& x_err_std, double& y_err_std);
protected:
	virtual void onTraceInitSuccess(TentativeTrack& trace);		//成功起始航迹
	virtual void onCancelTrace(TentativeTrack& trace);			//取消临时航迹

private:
	TraceIDType getNewID() { return ++globalTraceID; }
	MatrixXd GetP(const plotRecord& plot, double T);
	void clearUpTrace(int64_t current_timeSecFromEpoch, TraceInitSuccessMap_t& trace_sucess_map, DeleteTraceVec_t& delete_trace_vec);
	void tidyTrace(TraceInitSuccessMap_t& trace_sucess_map, DeleteTraceVec_t& delete_trace_vec);

	//从未分配的点迹中产生新的临时航迹
	void createNewTraceFromUnassignedPlot(const CCVBlockVec_t& plots,
		const std::vector<size_t>& assignedStatus,
		int64_t current_timeSecFromEpoch);

	//航迹分裂
	void traceSplit(TentativeTrack& trace, std::vector<plotRecord>& assignedPlotVector);

	//克隆上游航迹
	void createNewTraceFromCloneAndAddPlot(TraceIDType fromID, plotRecord& plot);

	//计算三个两个的航向夹角
	double calcIncludedAngle(
		const plotRecord& plot_0, //第一个点
		const plotRecord& plot_1, //第二个点
		const CCVBlockInfo_t& a_plot//第三个点
	);

	plotRecord buildOnePlotRecord(const CCVBlockInfo_t& a_plot,
		int64_t current_timeSecFromEpoch);

	//检查目标是否为杂波
	bool isClutter(TentativeTrack& trace);

	void initTrackFilter(double T);
private:
	TraceIDType					globalTraceID;
	TentativeTrackMap_t			Traces;
	TrackInitializeParam		trackParam;
	double						curSecsPreCycle;
	onTrackConfirmedAction		TrackConfirmedAct;
	FlickerTargetDetector		FlickerDetector;
};

#endif